/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-07-15 17:24:23
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-17 19:40:13
 * @FilePath: /esp32_motor/motor/inc/math_value.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#ifndef MATH_CONMON_H
#define MATH_CONMON_H

#include <motor.h>

#define _2_SQRT3                  1.15470053838f
#define _SQRT3                    1.73205080757f
#define _1_SQRT3                  0.57735026919f
#define _SQRT3_2                  0.86602540378f
#define _SQRT2                    1.41421356237f
#define _120_D2R                  2.09439510239f
#define _PI                       3.14159265359f
#define _PI_2                     1.57079632679f
#define _PI_3                     1.0471975512f
#define _2PI                      6.28318530718f
#define _3PI_2                    4.71238898038f
#define _PI_6                     0.52359877559f
#define _RPM_TO_RADS              0.10471975512f
#define MIN_ANGLE_DETECT_MOVEMENT (_2PI / 101.0f)
#define MOTOR_CYCLE               200//us


extern const _iq24 _2pi;
extern const _iq24 _1_sqrt3;
extern const _iq24 _sqrt3;
extern const _iq24 _2_sqrt3;
extern const _iq24 _half;
extern const _iq24 _3pi_2;
extern const _iq24 _pi_2;
extern const _iq24 _one;
extern const _iq24 _zero;
extern const _iq24 _min_angle_detect_movement;

#define CONSTRAIN(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#define MAX(a, b)                 (((a) > (b)) ? (a) : (b))
#define MIN(a, b)                 (((a) < (b)) ? (a) : (b))
#define ABS(A)                    (((A) >= 0) ? (A) : -(A))// 绝对值函数
#define UTILS_IS_NAN(x)           ((x) != (x))
#define UTILS_NAN_ZERO(x)         (x = UTILS_IS_NAN(x) ? 0 : x)
// #define NORM2_F(x, y)              (sqrt(pow(x, 2) + pow(y, 2)))

typedef struct
{
    _iq24 a;
    _iq24 b;
    _iq24 c;
    _iq24 q;
    _iq24 d;
    _iq24 alpha;
    _iq24 beta;
} PhasePara;

typedef struct
{
    int   meanWinSize;
    _iq24 lowPassAlpha;
    _iq24 ts;
} FilterCfg;

typedef struct
{
    _iq24 sinValue;
    _iq24 cosValue;
} SinCos;

typedef struct FilterAlgorithm
{
    void *priv;
    /*算法对象  滤波数据*/
    _iq24 (*slideMeanFilter)(struct FilterAlgorithm *, _iq24);
    /*算法对象  滤波数据*/
    _iq24 (*lowPassFilter)(struct FilterAlgorithm *, _iq24);
} FilterAlgorithm;

static inline void TruncateNumberAbs(_iq *number, _iq max)
{
    if (*number > max)
    {
        *number = max;
    }
    else if (*number < -max)
    {
        *number = -max;
    }
}

// static inline void TruncateNumber(double *number, double min, double max)
// {
//     if (*number > max)
//     {
//         *number = max;
//     }
//     else if (*number < min)
//     {
//         *number = min;
//     }
// }

// int16_t   ElectricalQAngle(int16_t qAngle);
esp_err_t FILTER_creat(FilterAlgorithm **alg, FilterCfg cfg);
void      FILTER_delete(FilterAlgorithm *alg);
// void      SlowSinCos(int16_t qAngle, SinCos *out);
void FastSinCos(_iq24 qAngle, SinCos *out);
// int16_t   FastGetAngelFromPhase(double alpha, double beta);
_iq24 ElectricalIQAngle(_iq24 qAngle);
void  ParkTrans(PhasePara *para, SinCos value);
void  ParkReversalTrans(PhasePara *para, SinCos value);
void  ClarkTrans(PhasePara *para);
void  ClarkReversalTrans(PhasePara *para);

#endif
